#pragma once

#include <QThread>
#include <QString>
#include <QObject>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include "rviz_custom_plugins/utils/bag_map_notifier.h"


namespace rviz_custom_plugins
{

namespace utils
{

class BagMapLoader: public QThread
{
	Q_OBJECT

public:
	BagMapLoader(QString& path, QObject *parent = nullptr);

	void run() override;

signals:
    // void loadPointCloudFinished(sensor_msgs::msg::PointCloud2::SharedPtr pcd,
    //     pcl::PointCloud<pcl::Normal>::Ptr normals);
    void pointCloudLoaded(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdPointNormal);

private:
	QString bagPath;
	BagMapNotifier* notifier;

	sensor_msgs::msg::PointCloud2 transformPointCloud(
    	sensor_msgs::msg::PointCloud2 &pointCloud2,
    	geometry_msgs::msg::PoseStamped &poseStamped);

	pcl::PointCloud<pcl::PointXYZI>::Ptr transformPointCloudV2(
		sensor_msgs::msg::PointCloud2& input_cloud,
		geometry_msgs::msg::PoseStamped& poseStamped);

    pcl::PointCloud<pcl::Normal>::Ptr estimateNormalsFromViewpoints(
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
        pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoints,
        int kSearch = 4);
	
	pcl::PointCloud<pcl::Normal>::Ptr estimateNormalsFromViewpoint(
		pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
		pcl::PointXYZ& viewpoint,
		int kSearch = 4);

    // pcl::PointCloud<pcl::PointXYZI>::Ptr voxelizePcd(const pcl::PointCloud<pcl::PointXYZI>::Ptr pcdIn, const float voxelRes);

	pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelizePcd(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr pcdIn, const float voxelRes);

	pcl::PointCloud<pcl::PointXYZINormal>::Ptr mergePointCloud(std::vector<sensor_msgs::msg::PointCloud2>& mapPointCloudVector,
		std::vector<geometry_msgs::msg::PoseStamped>& mapPoseVector);
};

}

}
